package Interface;

import lejos.nxt.*;
import lejos.nxt.comm.*;
import lejos.robotics.navigation.TachoPilot;

import java.io.*;
import java.util.ArrayList;

public class BluetoothNXT extends Thread {

	DataOutputStream dos;
	DataInputStream dis;
	// keep al data here
	int sonarDistance;
	byte barcode;
	byte color = (byte)0;
	int lightValue;
	int leftMotorspeed;
	int rightMotorspeed;
	boolean running = true;
	private TachoPilot pilot;
	private LightSensor lightsensor;
	private UltrasonicSensor sonicsensor;
	private int marginLow = 30;
	private int marginHigh = 30;
	private boolean panic;
	
	public enum ID{
	LEFTMOTOR ((byte)0),
	RIGHTMOTOR ((byte)1),
	LIGHTVALUE ((byte)2),
	COLOR ((byte)3), // zwart,hout wit <=> 0,1,2
	SONAR ((byte)4),
	BARCODE ((byte)5),
	PANEL ((byte)7);
	
	private final byte value;
	ID(byte value){
		this.value = value;
	}		
		public byte value() { return value; }
		
	}
	
	public enum Barcode {
		FORWARD (1),
		RIGHTTURN (6),
		LEFTTURN (3);
		
		  private final int value; // int
		    Barcode(int value) {
		        this.value = value;
		        
		    }
		    public int value()   { return value; }
	}
	
	
	public BluetoothNXT(TachoPilot pilot){
		this.pilot = pilot;
		this.lightsensor = new LightSensor(SensorPort.S1);
		this.sonicsensor = new UltrasonicSensor(SensorPort.S2);
		LCD.drawString("Waiting for connection",0,0);
		LCD.refresh();

        BTConnection btc = Bluetooth.waitForConnection();
        
		LCD.clear();
		LCD.drawString("connected...",0,0);
		LCD.refresh();	
		
		dos = btc.openDataOutputStream();
		dis = btc.openDataInputStream();
		panic = false;
	}
	
	public BluetoothNXT(TachoPilot pilot, LightSensor light, UltrasonicSensor sonar){
		this.pilot = pilot;
		this.lightsensor = light;
		this.sonicsensor = sonar;
		LCD.drawString("Waiting for connection",0,0);
		LCD.refresh();

        BTConnection btc = Bluetooth.waitForConnection();
        
		LCD.clear();
		LCD.drawString("connected...",0,0);
		LCD.refresh();	
		
		dos = btc.openDataOutputStream();
		dis = btc.openDataInputStream();
		panic = false;
	}
	
	public BluetoothNXT(TachoPilot pilot, LightSensor light, int marginLow, int marginHigh, UltrasonicSensor sonar){
		this.pilot = pilot;
		this.lightsensor = light;
		this.sonicsensor = sonar;
		this.marginLow = marginLow;
		this.marginHigh = marginHigh;
		LCD.drawString("Waiting for connection",0,0);
		LCD.refresh();

        BTConnection btc = Bluetooth.waitForConnection();
        
		LCD.clear();
		LCD.drawString("connected...",0,0);
		LCD.refresh();	
		
		dos = btc.openDataOutputStream();
	}
	
	private void sendValue(Byte id,int value){
		try {
			dos.write(id);
			dos.writeInt(value);
			dos.flush();
		} catch (IOException e) {			
			System.out.println(e.getMessage());
		}		
	}
	
	public void end(){
		running = false;
	}
	private void sendValue(Byte id,byte value){
		try {
			dos.write(id);
			dos.write(value);
			dos.flush();
		} catch (IOException e) {
			System.out.println(e.getMessage());
			
		}
		
	}
	
	
	
	public void setBarcode(int code){
		sendValue(ID.BARCODE.value(),(byte)code);
	}
	
	public void sendPanel(int panel){
	sendValue(ID.PANEL.value(),(byte)panel);
	}
	
	public void run(){
		
		while(running){
			if (rightMotorspeed != pilot.getRightActualSpeed()){
				rightMotorspeed = pilot.getRightActualSpeed();
				sendValue(ID.RIGHTMOTOR.value(),rightMotorspeed);
			}
			if (leftMotorspeed != pilot.getLeftActualSpeed()){
				leftMotorspeed = pilot.getLeftActualSpeed();
				sendValue(ID.LEFTMOTOR.value(),leftMotorspeed);
			}
			if (lightValue != lightsensor.getLightValue()){
				lightValue = lightsensor.getLightValue();
				sendValue(ID.LIGHTVALUE.value(),lightValue);
			}
			if (sonarDistance != sonicsensor.getDistance()){
				sonarDistance = sonicsensor.getDistance();
				sendValue(ID.SONAR.value(),sonarDistance);
			}
			if (color != getColorFromValue(lightsensor.getLightValue())){
				color = getColorFromValue(lightsensor.getLightValue());
				sendValue(ID.COLOR.value(),color);
			}
			
			try {
				Thread.sleep(250);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				System.out.println(e.getMessage());
			}
			
			try{
				if (!(dis.readByte() == 0)) panic = true;
			}catch(Exception e){
				//NOP
			}
			
		}
		
	}
	
	public boolean isPanic()
		{
		//panic = false;
		return panic;
		}
	
	public void setPanic(boolean panic){
	this.panic = panic;
	}
	
	/**
	 * Returns whether the detected lightvalue is black (2), white(0) or
	 * neutral(1)
	 */
	private  byte getColorFromValue(int value) {
		// Detected white
		if (value < 0 + marginLow)
			return (byte)2;
		// Detected black
		if (value > 100 - marginHigh)
			return (byte)0;
		else
			return (byte)1;
	}
	
	public void sendIntegers(ArrayList<Integer> list){
		try {
			dos.write((byte) 6);
		} catch (IOException e1) {
			// TODO Auto-generated catch block
			System.out.println(	e1.getMessage());
		}
		
		for (Integer integer : list) {
			try {
				dos.writeInt(integer);
			} catch (IOException e) {
				// TODO Auto-generated catch block
			System.out.println(	e.getMessage());
			}
		}
		
		try {
			dos.flush();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			System.out.println(	e.getMessage());
		}
	}
	
}
